/**
  ******************************************************************************
  * @file           : yaw_task.cpp
  * @author         : Tessia
  * @brief          : None
  * @attention      : None
  * @date           : 2024/2/26
  ******************************************************************************
  */



#include "yaw_task.h"

extern rc_device_t g_rc;
extern INS_t INS;
extern motion_type motionType;
extern float chassis_real_yaw;
extern receive_judge_t judge_rece_mesg;
extern uint8_t is_flip_over;
extern float spin_omega;

float yaw_set_gyro = 0;
float yaw_set_offset = 0;
float yaw_gyro = 0; //归一化
float yaw_gyro_filtered = 0;
float yaw_total_rounds = 0;
float yaw_current_rounds = 0;

float speed_filtered = 0;
float speed_current = 0;
float filtering_alpha = 0.01;
float speed_filtered_tmp = 0;
float chassis_gyro = 0;

uint8_t mains_power_gimbal_output_last = 0;
uint8_t mains_power_gimbal_output = 0;
uint8_t enable_cnt = 0;
uint32_t yaw_msg_cnt = 0;
uint32_t yaw_msg_cnt_last = 0;
uint8_t yaw_enable_flag = 0;

void yaw_task(void *argument){
    static pid yaw_gyro_ctrl(1.0f, 0.015, 0.f, 0.00f, 30.f);  //陀螺仪做位置控制
    static pid yaw_speed_filtered_ctrl(1.0f, 0.015, 0.f, 0.0001, 20.f);  //速度滤波，本来应该改电机底层，时间紧迫
    static DM_motor yaw_motor(&hcan2, nullptr, 0x000, 0x001, DM_J4310_2EC, DM_MIT, 1);
    yaw_gyro_ctrl.pid_set_variable_param(1.5 ,0, 0.1);
    yaw_speed_filtered_ctrl.pid_set_variable_param(8, 0, 0.1);

    yaw_motor.motor_control(DM_disable_offset);
    osDelay(5); //等待offset更新
    yaw_gyro = chassis_real_yaw / 360.f;
    chassis_gyro = yaw_gyro - (yaw_motor.motor_get_rounds() - YAW_FORWARD_ROUNDS);
    yaw_set_gyro = yaw_gyro;
    for(;;){
        yaw_gyro = chassis_real_yaw / 360.f;
        chassis_gyro = yaw_gyro - (yaw_motor.motor_get_rounds() - YAW_FORWARD_ROUNDS);
        low_pass(yaw_gyro_filtered, yaw_gyro, 0.03);
        yaw_total_rounds = yaw_motor.motor_get_rounds();
        yaw_current_rounds = yaw_motor.motor_get_current_rounds();
        low_pass(speed_filtered_tmp, yaw_motor.motor_get_speed(), filtering_alpha); //滤波
        speed_filtered = abs(speed_filtered_tmp) < 0.015 ? 0 : speed_filtered_tmp;
        speed_current = yaw_motor.motor_get_speed();

        yaw_motor.motor_error_protection();
        mains_power_gimbal_output_last = mains_power_gimbal_output;
        mains_power_gimbal_output = judge_rece_mesg.robot_status_data.mains_power_gimbal_output;
        yaw_msg_cnt_last = yaw_msg_cnt;
        yaw_msg_cnt = yaw_motor.motor_msg_cnt_return();

        if(mains_power_gimbal_output == 1 && mains_power_gimbal_output_last == 0){
            yaw_enable_flag = 1;
            while(1){
                yaw_motor.motor_set_current(0);
                yaw_msg_cnt_last = yaw_msg_cnt;
                yaw_msg_cnt = yaw_motor.motor_msg_cnt_return();
                if(yaw_msg_cnt != yaw_msg_cnt_last){
                    break;
                }
                osDelay(1);
            }
            osDelay(10);
            while(enable_cnt < 10){
                yaw_motor.motor_control(enable_motor);
                enable_cnt++;
                osDelay(1);
            }
            enable_cnt = 0;
        }
        yaw_enable_flag = 0;


        if(g_rc.is_lost == 0 && is_flip_over == 0){

            yaw_set_gyro = yaw_set_gyro - ((g_rc.rc_info.sw.sw_r == RC_SW_UP && g_rc.rc_info.sw.sw_l != RC_SW_MID) ? g_rc.rc_info.rocker.right_x / 2500.0 : 0);
            low_pass(yaw_set_offset, motionType == gyroscope || motionType == auto_navigate ? -0.16 / 0.7 * spin_omega : 0, 0.005);

            yaw_gyro_ctrl.pid_calculate(yaw_set_gyro + yaw_set_offset, yaw_gyro_filtered);
            yaw_speed_filtered_ctrl.pid_calculate(yaw_gyro_ctrl.out, speed_filtered);
            yaw_motor.motor_set_current(yaw_speed_filtered_ctrl.out);
        }else{
            yaw_set_gyro = yaw_gyro;
            yaw_motor.motor_set_current(0);
        }
        osDelay(1);
    }
}